#include <math.h>
#include <string.h>

#include "trace.h"
#include "common.h"

/* 轨迹初始化 */
int32_t Trace_Cycloid::trace_init()
{
    this->cycld_param.r = ((double *)get_trace_param())[0];
    this->cycld_param.w = ((double *)get_trace_param())[1];
    this->cycld_param.x_start = ((double *)get_trace_param())[2];
    this->cycld_param.y_start = ((double *)get_trace_param())[3];
    this->cycld_param.z_start = ((double *)get_trace_param())[4];
    this->pos[0] = this->cycld_param.x_start;   
    this->pos[1] = this->cycld_param.y_start;
    this->pos[2] = this->cycld_param.z_start;
    return 0;
}

int32_t Trace_Cycloid::trace_param_sync(void *param)
{
    double *tmp = (double *)param;

    memcpy(get_trace_param(), param, sizeof(Trace_Cycloid_Param));
    this->cycld_param.r = tmp[0];
    this->cycld_param.w = tmp[1];
    this->cycld_param.x_start = tmp[2];
    this->cycld_param.y_start = tmp[3];
    this->cycld_param.z_start = tmp[4];

    this->pos[0] = this->cycld_param.x_start;   
    this->pos[1] = this->cycld_param.y_start;
    this->pos[2] = this->cycld_param.z_start;

    return 0;
}

/* 轨迹退出 */
int32_t Trace_Cycloid::trace_deinit()
{
    return 0;
}

/* 摆线轨迹生成 
 * 前进走上面摆线，回去走下面直线，定义两个路程的时间相同
 **/
uint32_t Trace_Cycloid::trace_generate(double &time)
{
    double angle;   //弧度
    static uint32_t cnt = 0;    //摆动次数

    static double max_x = this->cycld_param.x_start + 2*PAI*this->cycld_param.r;
    static double time_half = (2*180.0f/this->cycld_param.w);
    this->pos[1] = this->cycld_param.y_start;        //y始终为不变
    if(time < time_half) {   //前半程
        angle = (time*this->cycld_param.w)*PAI/180.0f;
        this->pos[0] = this->cycld_param.r*(angle - sin(angle)) + this->cycld_param.x_start;
        this->pos[2] = this->cycld_param.r*(1 - cos(angle)) + this->cycld_param.z_start;    
    } else if((time > time_half) && (time < 2*time_half)){ //后半程
        this->pos[0] = max_x - (2*PAI*this->cycld_param.r*(time-time_half)/time_half);
        this->pos[2] = this->cycld_param.z_start;
    } else {
        time = 0;
        this->pos[0] = this->cycld_param.x_start; 
        this->pos[2] = this->cycld_param.z_start;
        cnt++;
    }
    
    return cnt;
}